import sys
from functools import lru_cache

from scipy.optimize import minimize

from tma.utils import settings as s
from tma.utils.custom_types import t_line, t_lines, t_point
from tma.utils.tools import *

# import math as m
# from typing import Optional, Union
# import cv2
# import numpy as np
# import sympy as sp
# from rich import print
# from scipy.linalg import lstsq


def cal_track_info(
    track_distance: float, bl: t_lines, cross_line: t_line, bl_time_tick: list[int]
) -> tuple[float, list, tuple[float, float]]:
    """give a track distance, and bearing lines info. find the smallest_error point"""
    _, try_point = evaluate_track(track_distance, bl, cross_line, bl_time_tick)
    track_fitness_error_collection = {}
    smallest_error = sys.maxsize
    try_step = -20
    while try_step < 20:
        try_step += 1
        try_point = (try_point[0] + try_step, calculate_y_coordinate(bl[0], try_point[0] + try_step))
        ok, p1, p2 = find_points_on_line_by_distance(try_point, bl[-1], track_distance)
        if not ok:
            continue
        p1_error = cal_track_fitness_metric(try_point, p1, bl, bl_time_tick)
        p2_error = cal_track_fitness_metric(try_point, p2, bl, bl_time_tick)

        if p1_error <= p2_error:
            track_fitness_error_collection[p1_error] = (try_point, p1)
        else:
            track_fitness_error_collection[p2_error] = (try_point, p2)

        smallest_error = min(p1_error, p2_error, smallest_error)

    if not track_fitness_error_collection:
        raise Exception("track_fitness_error compare error!")

    (x1, y1), (x2, y2) = track_fitness_error_collection[smallest_error]
    return (smallest_error, [x1, y1, x2, y2], cal_line_angle([x1, y1, x2, y2]))


###
### wrap function
###
def find_points_on_line_by_distance(p0: t_point, line: t_line, distance: float | int) -> tuple[bool, t_point, t_point]:
    """
    give a point p, a line l and a value v
    d =  perpendicular distance of p to l
    if d < v, return p1 and p2, where p1->p p2->p == d
    """
    return cached_find_points_on_line_by_distance(tuple(p0), tuple(line), distance)


def cal_try_point_error(
    try_point: t_point,
    lines_used: t_lines,
    travel_distance_used: float,
    bl_time_tick: list[int],
) -> float:
    # print('-'*55)
    # print(try_point)
    # print(lines_used[-1])
    # print(travel_distance_used)
    ok, p1, p2 = find_points_on_line_by_distance(try_point, lines_used[-1], travel_distance_used)
    if not ok:
        # raise Exception("cal_try_point_error() error!")
        return 99999

    p1_error = cal_track_fitness_metric(try_point, p1, lines_used, bl_time_tick)
    p2_error = cal_track_fitness_metric(try_point, p2, lines_used, bl_time_tick)

    return min(p1_error, p2_error)


def evaluate_track(
    track_distance: float, bl: t_lines, cross_line: t_line, bl_time_tick: list[int]
) -> tuple[float, t_point]:
    """give track distance, and bearing lines info, calculate track error uing minimize()"""

    return _cached_evaluate_track(
        track_distance,
        tuple((tuple(i) for i in bl)),
        tuple(cross_line),
        tuple(bl_time_tick),
    )


def cal_track_fitness_metric(
    head_point: t_point,
    tail_point: t_point,
    lines_used: t_lines,
    bl_time_tick: list[int],
) -> float:
    """evaluate track fitness"""
    return _cached_cal_track_fitness_metric(
        tuple(head_point), tuple(tail_point), tuple((tuple(i) for i in lines_used)), tuple(bl_time_tick)
    )


@lru_cache(1024)
def cached_find_points_on_line_by_distance(
    p0: t_point, line: t_line, distance: float | int
) -> tuple[bool, t_point, t_point]:
    """
    give a point p, a line l and a value v
    d =  perpendicular distance of p to l
    if d < v, return p1 and p2, where p1->p p2->p == d
    """
    x0, y0 = p0
    x1, y1, x2, y2 = line

    d_p2l = cal_perpendicular_distance(p0, line)
    if d_p2l > distance:
        return False, (0, 0), (0, 0)

    if x2 == x1:
        # print('x2 = = x1')
        dy = m.sqrt(distance**2 - (x0 - x1) ** 2)
        return True, (x1, y0 + dy), (x1, y0 - dy)

    else:
        x0, y0 = find_vertical_point(line, p0)
        d_shift = m.sqrt(distance**2 - d_p2l**2)
        slope, _ = get_line_slope_and_intercept(line)

        # dx ** 2 + (slope * dx) ** 2 = d_shift ** 2
        # dx ** 2 =  d_shift ** 2 / (1 + slope ** 2)
        dx = m.sqrt(d_shift**2 / (1 + slope**2))

        x1 = x0 + dx
        y1 = calculate_y_coordinate(line, x1)
        x2 = x0 - dx
        y2 = calculate_y_coordinate(line, x2)

        assert distance - distance_between_points(p0, [x1, y1]) < 1
        assert distance - distance_between_points(p0, [x2, y2]) < 1
        return True, (x1, y1), (x2, y2)


@lru_cache(10240)
def _cached_cal_track_fitness_metric(
    head_point: t_point,
    tail_point: t_point,
    bl: t_lines,
    bl_time_tick: list[int],
) -> float:
    """calculate track fit metric using postion-diff method"""
    assume_track_line = list(head_point) + list(tail_point)
    error_list = []
    points = divide_track_by_bl(assume_track_line, bl_time_tick)
    assert len(points) == len(bl)
    for point, line in zip(points, bl):
        error_list.append(distance_between_points(point, find_intersection_point(assume_track_line, line)))
    y_true = [0 for _ in range(len(bl))]
    y_pred = error_list
    distance = distance_between_points(head_point, tail_point) ** (1 / 2)
    return sum((yt - yp) ** 2 for yt, yp in zip(y_true, y_pred)) / (len(y_true) * distance)


# @lru_cache(10240)
def cached_cal_track_fitness_metric_02(
    head_point: t_point,
    tail_point: t_point,
    bl: t_lines,
    bl_time_tick: list[int],
    bl_orientation: list[float],
    own_pos: list[t_point],
) -> float:
    """calculate track fit metric using bearing angle-diff method"""
    assume_track_line = list(head_point) + list(tail_point)
    points = divide_track_by_bl(assume_track_line, bl_time_tick)
    assert len(points) == len(bl)
    total_error: list[Union[float, int]] = [0]
    for p1, p2, bearing in zip(own_pos, points, bl_orientation):
        # print(p1, p2, bearing)
        br = get_orientation_between_points(p1, p2)
        total_error.append(br - bearing)

    assert len(total_error) - len(own_pos) == 1
    return sum(total_error)


@lru_cache(10240)
def _cached_evaluate_track(
    track_distance: float, bl: t_lines, cross_line: t_line, bl_time_tick: list[int]
) -> tuple[float, t_point]:
    """give track distance, and bearing lines info, calculate track error uing minimize()"""
    outer_lines_cross_p = find_intersection_point(bl[0], bl[-1])
    if not outer_lines_cross_p:
        raise Exception("First and last bearring seems parallel")
    try_point = outer_lines_cross_p
    # point on x-axis where line pass it and view_center is vertical to first_line
    x_value, _ = find_intersection_point(cross_line, bl[0])

    if x_value < outer_lines_cross_p[0]:
        try_step = -s.Try_point_step
    elif x_value > outer_lines_cross_p[0]:
        try_step = s.Try_point_step
    else:
        # @TODO first_line is vertical to x-axis
        raise Exception("predict_target_track error: first_line is vertical to x-axis")

    try_x_bottom = try_point[0] + try_step
    while cal_perpendicular_distance(try_point, bl[-1]) <= track_distance:
        try_point = (
            try_point[0] + try_step,
            calculate_y_coordinate(bl[0], try_point[0] + try_step),
        )
    try_x_top = try_point[0] - try_step

    bounds = [(min(try_x_bottom, try_x_top), max(try_x_bottom, try_x_top)), (None, None)]
    res = minimize(cal_try_point_error, try_point, args=(bl, track_distance, bl_time_tick), bounds=bounds)

    # print("1" * 55)
    # print(res.success)
    # print(res.x)
    # print(res.fun)
    if res.success:
        return res.fun, res.x
    return _evaluate_track_partial_brute_force(bl, track_distance, cross_line, bl_time_tick)


@lru_cache(10240)
def _evaluate_track_partial_brute_force(
    bl: t_lines, track_distance: float, cross_line: t_line, bl_time_tick: list[int]
) -> tuple[float, t_point]:
    track_fitness_error_collection = {}
    outer_lines_cross_p = find_intersection_point(bl[0], bl[-1])
    if not outer_lines_cross_p:
        raise Exception("First and last bearring seems parallel")
    try_point = outer_lines_cross_p
    # point on x-axis where line pass it and view_center is vertical to first_line
    x_value, _ = find_intersection_point(cross_line, bl[0])

    if x_value < outer_lines_cross_p[0]:
        try_step = -s.Try_point_step
    elif x_value > outer_lines_cross_p[0]:
        try_step = s.Try_point_step
    else:
        # @TODO first_line is vertical to x-axis
        raise Exception("predict_target_track error: first_line is vertical to x-axis")

    smallest_error = sys.maxsize
    while cal_perpendicular_distance(try_point, bl[-1]) <= track_distance:
        try_point = (
            try_point[0] + try_step,
            calculate_y_coordinate(bl[0], try_point[0] + try_step),
        )
        ok, p1, p2 = cached_find_points_on_line_by_distance(try_point, bl[-1], track_distance)
        if not ok:
            break
        p1_error = cal_track_fitness_metric(try_point, p1, bl, bl_time_tick)
        p2_error = cal_track_fitness_metric(try_point, p2, bl, bl_time_tick)
        # if min(p1_error, p2_error) > smallest_error * 20:
        #     break

        if p1_error <= p2_error:
            track_fitness_error_collection[p1_error] = (try_point, p1)
        else:
            track_fitness_error_collection[p2_error] = (try_point, p2)

        smallest_error = min(p1_error, p2_error, smallest_error)

    return smallest_error, track_fitness_error_collection[smallest_error][0]
